#ifndef LIBCELL_POINTS_CLOUD_ALGORITHM1_H_
#define LIBCELL_POINTS_CLOUD_ALGORITHM1_H_

#ifndef SIGN
#define SIGN(x) ((x>0)?1:((x<0)?-1:0))
#endif SIGN

#include<Mesh/_Func_.h>
#include<tool/libcell_tools_algorithm.h>
#include<Mesh/libcell_iterator.h>
#include <Dense>
#include<SVD>
#include<tool/libcell_kd_node_iterator.h>
#include<iostream>

using namespace Eigen;
#define quote lib_cell_quote


// Eigen 默认是列储存，对于如下矩阵A
// Matrix A:
// 1 2 3
// 4 5 6
//A.data()的数据储存是如下
// Element 0: 1
// Element 1: 4
// Element 2: 2
// Element 3: 5
// Element 4: 3
// Element 5: 6

#ifdef __cplusplus
extern "C" {
#endif

static inline double compute_visual_avarage_distance(Node* n)
{
    double* box=mesh_compute_bounding_box_from_node(n);
    double min1=box[3]-box[0],min2=box[4]-box[1],min3=box[5]-box[2];
    double mid=min1>=min2?(min3>=min1?(min1):(min2>=min3?(min2):(min3))):(min3>=min2?(min2):(min1>=min3?(min1):(min3)));
    double vdis=mid/70.0;
    return vdis;
}


double*compute_pca_result_using_eigen(Node* n)
{
    int len=node_size(n);

    if(len<3)
    {
        return NULL;
    }
    template_v* v=(template_v*)(n->value);

    Eigen::MatrixXd A;
    A.resize(len,v->point_size);
    A.setZero();

    int i=0;
    for(Node* nit=n;nit!=NULL;nit=(Node*)(nit->Next))
    {
        v=(template_v*)(nit->value); 
        for(int j=0;j<v->point_size;j++)
        {
            A(i,j)=v->point[j];
        }
        i++;
    }
    Eigen::MatrixXd B=(MatrixXd::Ones(1,len)*A)*((1.0/(double)len));
    for(int i=0;i<len;i++)
    {
        A.row(i)=A.row(i)-B;
    }
    JacobiSVD<Eigen::MatrixXd> svd(A,ComputeThinU|ComputeThinV);
    MatrixXd V=svd.matrixV(); 
    //printf("%lf %lf\n",V(0,0),V(0,1));
    //printf("%d %d\n",V.rows(),V.cols());
    double* re=(double*)malloc(sizeof(double)*v->point_size*v->point_size); 

    memmove(re,V.data(),sizeof(double)*v->point_size*v->point_size);

    return re;
}

//double compute_appropriate_resolution_radius();

int is_boundary_vertex_in_points_cloud(KD_Node*tree,template_v* v,double r)
{
    Node* n=kd_tree_find_nearest_sphere(v,r,tree);
    double* pcas= compute_pca_result_using_eigen(n);
    int * grid=(int*)malloc(sizeof(int)*25);
    memset(grid,0,sizeof(int)*25);
    MatrixXd A;
    A.resize(v->point_size,v->point_size);
    memmove(A.data(),pcas,sizeof(double)*v->point_size*v->point_size);
    MatrixXd B=A.inverse();
    VectorXd C;C.resize(v->point_size,1); 
    double step=r/5.0;
     
    for(Node* nit=n;nit!=NULL;nit=(Node*)(nit->Next))
    {
        template_v* v1=(template_v*)(nit->value);
        for(int j=0;j<v->point_size;j++)
        {
            C(j)=v1->point[j]-v->point[j];
        }
        MatrixXd temp_m= (B*C);
        int x=0,y=0;
        x= (int)((fabs(temp_m(0))-step)/2.0*step);
        x<0?(x=0):(x=x+1);x>2?(x=2):(x=x);  
        y= (int)((fabs(temp_m(1))-step)/2.0*step);
        y<0?(y=0):(y=y+1);  y>2?(y=2):(y=y);
        x=SIGN(temp_m(0))*x;y=SIGN(temp_m(1))*y;
        x+=2;y+=2;
        grid[x*5+y]=1;
    }
    int re=0;
    int xs[2][2]={{0,1},{3,4}};

    for(int i=0;i<2;i++)
    {
        for(int j=0;j<2;j++)
        {
            if(grid[xs[i][0]*5+xs[j][0]]==0&& grid[xs[i][0]*5+xs[j][1]]==0 && grid[xs[i][1]*5+xs[j][0]]==0&& grid[xs[i][1]*5+xs[j][1]]==0)
            {
                re=1;
                break;
            }
        }
        if(re==1)
        {
            break;
        }
    }
    free(grid);
    free(pcas);
    free_node(n);
    return re;
}


void points_cloud_adjust_normals(Node* n);

//***********************
//这里是loop,也就是这个点集是有顺序的
//那么计算的法向也是有固定方向的
//***********************
#include<Algorithm/subdivision_of_polygon.h>

double * compute_3D_loop_normal(Node*n)
{
    double *axis=compute_pca_result_using_eigen(n);
    double *normal=(double*)malloc(sizeof(double)*3); 
    normal[0]=axis[6];normal[1]=axis[7];normal[2]=axis[8];
    free(axis);
    normalize(normal,3);
    int len=node_size(n);
    double ** vs=(double**)malloc(sizeof(double*)*len);
    
    Node*nit= n;
    for(int i=0;i<len;i++)
    {
        vs[i]=(double*)malloc(sizeof(double)*3);
        template_v* v=(template_v*)(nit->value);

        double fac=inner_product(v->point,normal,3);
        vs[i][0]=v->point[0]-fac*normal[0]; vs[i][1]=v->point[1]-fac*normal[1]; vs[i][2]=v->point[2]-fac*normal[2];


        nit=(Node*)(nit->Next);
    }
    double * re=compute_plane_normal_of_jordan_curve(vs,len);
    normalize(re,3);
    free(normal);
    for(int i=0;i<len;i++)
    {
        free(vs[i]);
    }
    free(vs);
    return re;
}



#ifdef __cplusplus
}
#endif
#undef quote
#endif
